导轨相位投影开发说明

1. 相位同步投影方案

下面以单自由度导轨说明

根据需求，用户指定起点和终点，包括协同路径的起点和终点，以及导轨的起点和终点。所以需要进行“相位同步”投影。将当前点距离协同路径的起点的弧长传入，然后通过弧长映射到导轨运动区间（用户指定的起点和终点）如图：

映射关系：

$s_{track} / L_{track} = s_{path} / L_{path}$

不改变现有接口，将TrackIKConfigInfo中添加两个成员变量，用户只需要设置track_range

struct TrackIKConfigInfo
{
    int       type{0};                    // 逆解策略类型(0: 垂直投影, 1: 相位投影)

    //! 垂直投影设置
    Array3d   offset_params{};            // 导轨求逆解时, 设置的偏移参数(给导轨末端添加一个box, 这里设置box的长宽高,对应{x, y, z});
    Array3d   track_ref{};                // 导轨的参考位置

    //! 相位投影设置
    std::array<Array3d, 2>          track_range{};            // 导轨运动区间
    double                          phase_scale;              // 相位投影比例参数
};

参数track_range，由用户指定，作为导轨运动区间，phase_scale由plan在规划阶段计算写入。

2. 导轨速度以及加速度限制

重载了现有接口tpAddPositionLine，通过urdf文件获取导轨的速度以及加速度限制, 规划内部以导轨速度以及加速度限制为基准，规划协同路径

3. 开发示例

//! step1: 实例robot
type.second = aubo_i10;
Setup(getRobotType(type.second));

//! step2: 标定工件坐标系{wp}相对于导轨基座标系{track}的位置
interface::RLPose F_B_track, F_B_wp, F_track_wp; // 通过机器人标定，获取F_B_track，F_B_wp
F_track_wp = getWPFrameInTrack(robot, F_B_wp, F_B_track); // 本实例假设工件坐标系和导轨坐标系完全重合

// 设置导轨
std::string temp_URDF_PATH = "test/data/tp/external_axle_coordinate/"; // 导轨urdf文件
robot->rlInitiateTrackMotionFromFiles("aubo_track_dof_01", temp_URDF_PATH.c_str(), F_track_wp);
int track_dof = 1;

// 构建路点数据
interface::RLJntArray q0; // 机械臂的初始位置
interface::RLJntArray maxJV = {10, 10, 10, 10, 10, 10};
interface::RLJntArray maxJA = {30, 30, 30, 30, 30, 30};
std::array<double, 2> maxCV = {10, 10};
std::array<double, 2> maxCA = {80, 80};

std::vector<interface::PathProperty> path_propertys;
std::vector<interface::MoveProperty> move_propertys;
std::vector<interface::PathPoint> pathpoint;
std::vector<interface::RLPose> poses;
std::vector<interface::TrackIKConfigInfo> track_configs;

q0 = {33.34 * M_PI / 180, -16.69 * M_PI / 180, 98.32 * M_PI / 180, 24.94 * M_PI / 180, 90.25 * M_PI / 180, 33.34 * M_PI / 180};
poses = { {0.6316567628115636,0.5334872319012133,0.5824663189182125,-3.134912569201568,0.004272085690594685,1.569044343608399},
         {0.6316567577716762,0.8401660046995325,0.5824663239968462,-3.134912877766409,0.004272106335617448,1.569044268191154} };

interface::TrackIKConfigInfo track_config;
track_config.type = 1;
track_config.track_range = { { {0.18, 0, 0}, {0.6, 0, 0} } }; // 设置导轨运动范围
track_configs.push_back(track_config);
track_config.track_range = { { {0.6, 0, 0}, {0.7, 0, 0} } }; // 设置导轨运动范围
track_configs.push_back(track_config);

int N = poses.size();
path_propertys.resize(N);
move_propertys.resize(N);
pathpoint.resize(N);

interface::Array2d blend_radius = {-0.1, -0.1}; // 不支持交融，设置为负数
std::vector<double> maxCV_main = {0.4, 0.4}; // 协同路径的速度
std::vector<double> maxCA_main = {2, 20};
std::vector<double> maxCJ_main = {16, 160};

for(int i = 0; i < N; i++)
{
    path_propertys[i].plan_method = interface::PlanMethod::ONLY_POSITION_PLAN;
    path_propertys[i].describe_space = interface::DescribeSpace::CARTESIAN;
    path_propertys[i].cur_prop.type = interface::CurveType::LINE;
    path_propertys[i].blend_radius = blend_radius;
    path_propertys[i].track_ik_config = track_configs[i];

    move_propertys[i].maxV = maxCV_main;
    move_propertys[i].maxA = maxCA_main;
    move_propertys[i].maxJ = maxCJ_main;
    move_propertys[i].dynamic_adjust_motion_constraints = false;
    move_propertys[i].sync_move = true;

    pathpoint[i] = generatePathPoint(interface::DescribeSpace::CARTESIAN, i+1, {}, poses[i]);
}

// 添加路点
initiatePlanner(robot, q0, t_w);
robot->tpSetVelocityAndAccelerationLimits(maxJV, maxJA, maxCV, maxCA, 0);
robot->tpSetPlannerCapacity(interface::PlannerCapacity::LookAhead, 1);
robot->tpSetPlannerCapacity(interface::PlannerCapacity::ExecutionTrajectory, 10000);

int ret = 0;
for(int i = 0; i < N; i++)
{
    ret = robot->tpAddPositionLine(pathpoint[i], path_propertys[i], move_propertys[i]);
    CHECK(ret >= 0);
}
robot->tpSetEndPath();

// 采样取点
std::vector<interface::TrajectoryPoint> traj_points;
int plan_ret = interface::E_PLN_OK;
double cycle = 0.005;
interface::TrajectoryPoint point;
while ((plan_ret = robot->tpUpdateCycle(cycle, point)) != interface::E_PLN_EXEC_EMPTY)
{
    if(plan_ret < interface::E_PLN_OK)
        return;
    traj_points.push_back(point);
}

// 由采样点求导轨逆解
std::vector<std::vector<double>> track_joints;
for(auto& point : traj_points)
{
    std::vector<double> track_out;
    ret = robot->kdCalTrackInverseKinematics(point.point.pose, point.track_ik_config, track_out);
    CHECK(ret >= 0);
    track_joints.push_back(track_out);
}

// 求机器人逆解
int sample_point_num = track_joints.size();
std::vector<std::vector<double>> track_pos(track_dof, std::vector<double>(sample_point_num));
for(int i = 0; i < track_dof; i++)
{
    for(int j = 0; j < sample_point_num; j++)
    {
        track_pos[i][j] = track_joints[j][i];
        LOGI(log_) << "导轨位置: " << track_joints[j] << ENDL(log_);
    }
}

calRobotTraj(robot, F_track_wp, track_pos, q0, traj_points);
plotCurve(track_pos, traj_points, traj_points);

//======================================= 示例封装的相关函数 =================================================
inline interface::PathPoint generatePathPoint(const interface::DescribeSpace& space, const double& id, const std::vector<double>& data)
{
    interface::PathPoint point;
    point.type  = space;
    if(space == interface::DescribeSpace::JOINT)
        memcpy(point.joint.data(), data.data(), sizeof (double) * point.joint.size());
    else
        memcpy(point.pose.data(), data.data(), sizeof (double) * point.pose.size());

    point.id = id;

    return point;
}

int calRobotTraj(const interface::ARALIntfacePtr& robot, const interface::RLPose& F_track_wp, const std::vector<std::vector<double>>& track_position, const interface::RLJntArray& q_ref, std::vector<interface::TrajectoryPoint>& colla_traj)
    {
        log_->setLogLevel(3);
        int ret = 0;
        interface::IKConfigInfo config;
        config.sol_tolerance = {1e-12, 1e-12};
        config.timeout = 200000;
        config.q_ref = q_ref; // 初始参考关节角

        int track_dof = track_position.size();
        int sample_point_num = colla_traj.size();
        std::vector<interface::TrajectoryPoint> robot_traj(sample_point_num); // 存储离线轨迹点
        std::vector<double> tmp_track_out(track_dof);
        for(int i = 0; i < sample_point_num; i++)
        {
            for(int j = 0; j < track_dof; j++)
                tmp_track_out[j] = track_position[j][i];

            interface::RLPose F_track_Base;
            ret = robot->kdCalTrackForwardKinematics(tmp_track_out, F_track_Base);
            colla_traj[i].point.t_w.workpiece = robot->kdChangeReferenceFrame(F_track_wp, robot->kdInverseFrame(F_track_Base));

            interface::RLPose point_in_base;
            point_in_base = robot->kdChangeReferenceFrame(colla_traj[i].point.pose, colla_traj[i].point.t_w.workpiece);
            config.goal = point_in_base;
            ret = robot->kdCalInversePosition(config, colla_traj[i].point.t_w.tool, true, colla_traj[i].point.joint);
            if(ret < 0)
            {
                LOGE(log_) << "robot->kdCalInversePosition, 出错!" << ENDL(log_);
                return ret;
            }
            config.q_ref = colla_traj[i].point.joint;
        }
        return ret;
    }